#include "zadacha.h"
#include "hessian.h"
#include "int2str.h"
#include "../parser/fparser.hh"
#include <fstream>
#include <iostream>

#define min(a,b) (a < b ? a : b)
#define max(a,b) (a > b ? a : b)


//исходный терминальный функционал
double F0 (std::valarray<double> x, input& data) {
  unsigned m = data.m;
  std::vector<double> vars;
  for (unsigned i=0; i<m; i++) {
    vars.push_back(x[i]);
  }

  double variables[vars.size()];
  for (unsigned i=0; i<vars.size(); i++) {
    variables[i] = vars[i];
  }

  std::vector<std::string> functions = data.functions;
  std::string parsx = data.parsx;

  parsx = parsx.substr(0,parsx.length()-1);
  //std::cout << parsx << std::endl;                                                                                                                          

  FunctionParser rez;
  //std::cout << functions.back() << std::endl;                                                                                                               
  rez.Parse(functions.back(), parsx);
  return rez.Eval(variables);
}


//правые части управляемой системы
std::valarray<double> right_parts (std::valarray<double> x, double t, std::valarray<double> u, input& data) {
  unsigned m = data.m;
  unsigned q = data.q;
  std::valarray<double> f(q);

  std::vector<double> vars;
  for (unsigned i=0; i<m; i++) {
    vars.push_back(x[i]);
  }

  for (unsigned i=0; i<u.size(); i++) {
    vars.push_back(u[i]);
  }

  vars.push_back(t);

  double variables[vars.size()];
  for (unsigned i=0; i<vars.size(); i++) {
    variables[i] = vars[i];
  }

  std::vector<std::string> functions = data.functions;
  std::string pars = data.pars;

  FunctionParser fp[m];
  for (unsigned i=0; i<m; i++) {
    fp[i].Parse(functions[i], pars);
    f[i] = fp[i].Eval(variables);
  }

  std::valarray<double> tmp(m);

  tmp = delta(x,data);
  for (unsigned i=m; i<q; i++) f[i] = tmp[i-m];
  return f;
}


//оштрафованный функционал
double I (std::valarray<double> x, input& data) {
    double beta0,
           P = 1,
           S = 0;
    unsigned m = data.m,                                    //размерность пространства x
             q = data.q;                                    //размерность пространства x и z
    std::valarray<double> beta(q), beta1(m), beta2(m), df(m), z(m);

    for (unsigned i=0; i<m; i++) z[i] = x[i+m];

    std::valarray<double> h(q);
    for (unsigned i=0; i<m; i++) h[i] = z[i];

    df = delta_f(x,data);
    for (unsigned i=m; i<q; i++) h[i] = df[i-m];

    bool ind = true;

    std::vector<unsigned> J0, J;
/*
    for (unsigned i=0; i<q; i++) {
        std::cout << "h[" << i << "]= " << h[i] << " ";
    }

    std::cout << std::endl;
*/
    for (unsigned i=0; i<q; i++) {
        if (h[i] <= 0.00001) J0.push_back(i);
        else {
            ind = false;
            J.push_back(i);
        }
    }

    if (ind == true) {           //J0 = {1,...,2n}
        beta0 = 1;
        beta1 = 0;
        beta2 = 0;
    }
    else {                       //J0 != {1,...,2n}
        beta0 = 0.01;
        //for (unsigned i=0; i<J0.size(); i++) beta[J0[i]] = 0;
        for (unsigned i=0; i<J.size(); i++) P *= h[J[i]];
        for (unsigned i=0; i<J.size(); i++) S += P/h[J[i]];
        for (unsigned i=0; i<J.size(); i++) beta[J[i]] = P/S/h[J[i]];
        for (unsigned i=0; i<m; i++) beta1[i] = beta[i];
        for (unsigned i=0; i<m; i++) beta2[i] = beta[i+m];
    }

    data.beta0 = beta0;
    for (unsigned i=0; i<m; i++) data.beta1[i] = beta1[i];
    for (unsigned i=0; i<m; i++) data.beta2[i] = beta2[i];
    for (unsigned i=0; i<m; i++) data.df[i] = df[i];
    data.F = F0(x,data);

    return beta0*F0(x,data) + product_vec(beta1,z) + product_vec(beta2,df);
}


//отклонение в финальный момент времени
std::valarray<double> delta_f (std::valarray<double> x, input& data) {
    unsigned m = data.m;
    std::valarray<double> df(m), xlf(m), xuf(m), ind_xlf(m), ind_xuf(m);
    xlf = data.xlf;
    xuf = data.xuf;
    ind_xlf = data.ind_xlf;
    ind_xuf = data.ind_xuf;

    for (unsigned i=0; i<m; i++) {
        df[i] = ((ind_xlf[i]==1) ? pow(min(0,x[i]-xlf[i]),2) : 0)  + ((ind_xuf[i]==1) ? pow(max(0,x[i]-xuf[i]),2) : 0);
/*        if (df[i]>0.001) {
            std::cout << "delta_f" << srd::endl;
            std::cout << "x_f[" << i << "]= " << x[i] << " ";
            std::cout << std::endl; std::cout << std::endl;
        }
*/    }
    return df;
}

//отклонение внутри временного отрезка
std::valarray<double> delta (std::valarray<double> x, input& data) {
    unsigned m = data.m;
    std::valarray<double> d(m), xl(m), xu(m), ind_xl(m), ind_xu(m);
    xl = data.xl;
    xu = data.xu;
    ind_xl = data.ind_xl;
    ind_xu = data.ind_xu;

    for (unsigned i=0; i<m; i++) {
        d[i] = ((ind_xl[i]==1) ? pow(min(0,x[i]-xl[i]),2) : 0)  + ((ind_xu[i]==1) ? pow(max(0,x[i]-xu[i]),2) : 0);
/*        if (d[i]>0.001) {
            cout << "delta" << endl;
            cout << "x[" << i << "]= " << x[i] << " ";
            cout << endl;
        }
*/
    }
    return d;
}


void input::set_method(unsigned a, unsigned b) {
    tip_metoda = a;
    tip_integr = b;
}


void input::init(std::string file) {
    std::ifstream in(file.c_str());
    in >> m;                                               //размерность фазового пространства x

    q = 2*m;                                               //размерность всей системы (x и z)
    x.resize(q);

    //фазовые ограничения
    //------------------------------------------------------------------
    xl.resize(m);
    xu.resize(m);
    xlf.resize(m);
    xuf.resize(m);

    ind_xl.resize(m);
    ind_xu.resize(m);
    ind_xlf.resize(m);
    ind_xuf.resize(m);
    //------------------------------------------------------------------

    beta1.resize(m);
    beta2.resize(m);

    df.resize(m);

    for (unsigned i=0; i<m; i++) in >> x[i];               //значения на левом конце отрезка

    for (unsigned i=0; i<m; i++) in >> xl[i];
    for (unsigned i=0; i<m; i++) in >> ind_xl[i];

    for (unsigned i=0; i<m; i++) in >> xu[i];
    for (unsigned i=0; i<m; i++) in >> ind_xu[i];

    for (unsigned i=0; i<m; i++) in >> xlf[i];
    for (unsigned i=0; i<m; i++) in >> ind_xlf[i];

    for (unsigned i=0; i<m; i++) in >> xuf[i];
    for (unsigned i=0; i<m; i++) in >> ind_xuf[i];

    in >> t >> tf;                                          //временной отрезок
    in >> N;                                                //число шагов (интегрирования системы)
    in >> num_upr;                                          //число управлений в системе

    c.resize(num_upr);                                      //число точек переключения для каждого управления
    for (unsigned i=0; i<num_upr; i++) in >> c[i];

    sum_c =  c.sum();                                       //всего точек переключения для всех управлений

    std::vector<unsigned> tmp(num_upr+1);
    tmp[0]=0;

    const unsigned num_w = sum_c + num_upr;
    std::vector<double> niz(num_w);                         //нижнее ограничение на w_i
    std::vector<double> verh(num_w);                        //верхнее ограничение на w_i
    for (unsigned i=0; i<niz.size(); i++) in >> niz[i];
    for (unsigned i=0; i<verh.size(); i++) in >> verh[i];

    unsigned u_ind=0;
    switch (tip_metoda) {
        case 1:
               n = sum_c + num_upr;                         //размерность при кусочно-постоянном управлении
               for (unsigned i=0; i<num_w; i++) {
                   rgn.push_back(niz[i]);
                   rgn.push_back(verh[i]);
               }
               break;
        case 2:
               n = sum_c + 2*num_upr;                       //размерность при кусочно-линейном непрерывном управлении
               for (unsigned i=1; i<num_upr+1; i++) {
                   tmp[i] = ((c[i-1]+2)+tmp[i-1])-1;
               }

               for (unsigned i=0; i<num_w; i++) {
                   if (tmp[u_ind]==i) {
                       rgn.push_back(niz[i]);
                       rgn.push_back(verh[i]);
                       u_ind++;
                   }
                   rgn.push_back(niz[i]);
                   rgn.push_back(verh[i]);
               }
               break;
        case 3:
               n = 2*(sum_c + num_upr);                     //размерность при кусочно-линейном разрывном управлении
               for (unsigned i=0; i<num_w; i++) {
                   for (unsigned k=0; k<2; k++) {
                       rgn.push_back(niz[i]);
                       rgn.push_back(verh[i]);
                   }
               }
               break;
    }

    tau0.resize(sum_c);
    for (unsigned i=0; i<sum_c; i++) in >> tau0[i];         //начальные значения точек переключения

    rgn_tau.resize(2*sum_c);
    for (unsigned i=0; i<2*sum_c; i++) in >> rgn_tau[i];    //ограничения на область изменения для каждой точки переключения

    in >> eps;
    in >> epsilon;
    in >> tip;
    in >> integr;
    in >> delim;
    in.close();

    std::string system = "sys.txt";
    std::ifstream sys(system.c_str());
    std::string prav;
    functions.resize(m+1);     //правые части + функционал                                                                                                   $

    unsigned j = 0;
    while (getline(sys, prav)) {
      if (prav != "") {
        functions[j] = prav;
        j++;
      }
    }
    sys.close();

    //for (unsigned i=0; i<m+1; i++) {                                                                                                                       $
    //    std::cout << functions[i] << std::endl;                                                                                                            $
    //}                                                                                                                                                      $

    for (unsigned i=0; i<m; i++) {
      pars.append("x" + Int2Str(i) + ",");
    }

    parsx = pars;
    for (unsigned i=0; i<num_upr; i++) {
      pars.append("u" + Int2Str(i) + ",");
    }

    pars.append("t");
}
